
% close all
% plot(zpos.lhip(StartPt:EndPt),-ypos.lhip(StartPt:EndPt))
% hold on
% plot(zpos.lankle_avg(StartPt:EndPt),-ypos.lankle_avg(StartPt:EndPt),'r')
% plot(zpos.lankle_avg(StartPt),-ypos.lankle_avg(StartPt),'bs')
% plot(zpos.lhip(StartPt),-ypos.lhip(StartPt),'rs')
% label('x')


%%
plot(-zpos.lhip(StartPt:EndPt),ypos.lhip(StartPt:EndPt))
hold on
plot(-zpos.lankle_avg(StartPt:EndPt),ypos.lankle_avg(StartPt:EndPt),'r')
plot(-zpos.lankle_avg(StartPt),ypos.lankle_avg(StartPt),'bs')
plot(-zpos.lhip(StartPt),ypos.lhip(StartPt),'rs')
% label('x')

%%
plot(ns_time_mean,x_meanNSL)
hold on
plot(ns_time_mean,x_meanSL)
plot(ns_time_mean,x_meanTH)

%%
plot(-zpos.rhip(StartPt:EndPt),ypos.rhip(StartPt:EndPt))
hold on
plot(-zpos.rankle_avg(StartPt:EndPt),ypos.rankle_avg(StartPt:EndPt),'r')
plot(-zpos.rankle_avg(StartPt),ypos.rankle_avg(StartPt),'bs')
plot(-zpos.rhip(StartPt),ypos.rhip(StartPt),'rs')

%% generate the figures
% stance ankle
% OptFit2 locates at ./Research/output_3/Plot_func/
load('./data_HM_3D/Afit.mat')
OptFit2(ave_time,upperBSA,lowerBSA,x_meanSA,'sa3D','Angle (rad)',a0(1,:),2,ns_time_mean,'$\theta_{sa}^H$','$\theta_{sa}^f$')
%%
OptFit2(ave_time,upperBHipz,lowerBHipz,x_meanHipz,'hipy','Position (m)',a0(2,:),2,ns_time_mean,'$p_{hipy}^H$','$p_{hipy}^f$',[-0.1 0.1])
OptFit2(ave_time,upperBTorso,lowerBTorso,x_meanTorso,'torso3D','Angle (rad)',...
    a0(3,:),2,ns_time_mean,'$\theta_{nship}^H$','$\theta_{nship}^f$',[1.3 1.8])

%% both knee
OptFit2(ave_time,upperBSK,lowerBSK,x_meanSK,'SKnee','Angle (rad)',a0(3,:),2,ns_time_mean,'$\theta_{sknee}^H$','$\theta_{sknee}^f$',[-0.1 0.8])
OptFit2(ave_time,upperBNSK,lowerBNSK,x_meanNSK,'NSKnee','Angle (rad)',a0(4,:),2,ns_time_mean,'$\theta_{nsknee}^H$','$\theta_{nsknee}^f$',[-0.1 1.4])
%% non-stance slope
x_meanNSL = Xmean.x_meanNSL;
upperBNSL = XupperB.upperBNSL;
lowerBNSL = XlowerB.lowerBNSL;
OptFit2(ave_time,upperBNSL,lowerBNSL,x_meanNSL,'NSSlope','Slope',a0(2,:),2,ns_time_mean,'$m_{nsl}^H$','$m_{nsl}^f$',[-0.6 0.8])
%% hip position
x_meanHP = Xmean.x_meanHP;
upperBHP = XupperB.upperBHP;
lowerBHP = XlowerB.lowerBHP;
OptFit2(ave_time,upperBHP,lowerBHP,x_meanHP,'HipPosition','Position (m)',a0(1,:),1,ns_time_mean,'$p_{hip}^H$','$p_{hip}^f$',[0 0.8])

%%
OptFit2(ave_time,upperBNSHipy,lowerBNSHipy,x_meanNSHipy,'NSHipy','Position (m)',a0(2,:),2,ns_time_mean,'$p_{nshipy}^H$','$p_{nshipy}^f$',[-0.1 0.1])

%%
OptFit2(ave_time,upperBTH,lowerBTH,x_meanTH,'Torso','Angle(rad)',a0(5,:),2,ns_time_mean,'$\theta_{torso}^H$','$\theta_{torso}^d$',[-0.2 0.4])